function curEst = EstimateState( allPts, allCam, bugmodel, prevEst, Truth )
dbstop if error
% compute an estimate of the current state
% inputs:
% allPts: the projections of 3D surface point cloud onto some image planes
% allCam: all of the transformations of the camera that creates the points
% bugmodel: defines the creation of 3D point clouds given position in state
% space
% prevEst: the previous estimates of position in state space
% Truth: if using simulated data, put in truth so we can verify that we
% match it after convergence
converged = 0;
curEst = prevEst(:,size(prevEst,2)); 
Ncams = length(allCam);
% start with the the last estimate as a guess
% a better guess would be a (linear?) predictor based on several previous
% states
[curPts depInfo] = bugmodel.getPointCloudFromState( curEst );

while (~converged)
    
    ptDists = zeros(Ncams,1); % a distance for each of the camera views
    
    for k = 1:Ncams
        disp(['matching camera ' num2str(k) ' of ' num2str(Ncams)])
        % for each dependency group of parameters
        for pidx = 1:length(depInfo)
            disp(['matching parameters set ' num2str(pidx) ' of ' num2str(length(depInfo))])
            group = depInfo{pidx};
            subconverged = 0;
            Sigma = diag( abs(curEst(group)) );
            h = .05;
            minE = 1e3;
            samps = 1;
            while( ~subconverged )

                curEst_ = curEst;
                delta = SampleNDimGaussian(curEst_(group),Sigma*h);
                curEst_(group) = delta;
                    
                % obtain the measurement- the point cloud of the bug model
                if( samps > 1 )
                curPts = bugmodel.getPointCloudFromState( curEst_ );
                else
                    curPts = bugmodel.getPointCloudFromState( curEst );
                end
                
                % the target points that we'd like to match to from the k-th view
                targetImgPts = allPts{k};
                tarX = targetImgPts(:,1);
                tarY = targetImgPts(:,2);

                % the camera transformation of the k-th view
                camA = allCam{k};
                ptsCam = camA * [curPts; ones(1,size(curPts,2))];
                PI = [eye(3,3) zeros(3,1)];
                lptsImg = PI*ptsCam;
                % the x and y image points from current estimate
                % we need to find the transformation in state space
                % that matches these with tarX and tarY
                ptsX = lptsImg(1,:)./lptsImg(3,:);
                ptsY = lptsImg(2,:)./lptsImg(3,:);

                dist = ComputeHausDist( tarX(:), tarY(:), ptsX(:), ptsY(:) );

                if( dist < minE )
                    minE = dist
                    h = h/2;
                    plot(ptsX,ptsY,'.r'); axis([-0.5 0.5 -0.5 0.5]);
                    hold on
                    plot(tarX,tarY,'.b'); axis([-0.5 0.5 -0.5 0.5]);
                    hold off
                    if( samps > 1 )
                        curEst = curEst_;
                    end
                end                
                              
                samps = samps + 1;
                if( samps > 30 )
                    subconverged = 1;
                end
            
            end
            
        end
        
    end
    
    
    converged = 1;
    
    
    
    
end;